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Abstract 

This paper proposes a design methodology for non-linear state ob- 
servers for invariant kinematic systems posed on finite dimensional con- 
nected Lie groups, and studies the associated fundamental system struc- 
ture. The concept of synchrony of two dynamical systems is specialised 
to systems on Lie groups. For invariant systems this leads to a general 
factorisation theorem of a nonlinear observer into a synchronous (inter- 
nal model) term and an innovation term. The synchronous term is fully 
specified by the system model. We propose a design methodology for 
the innovation term based on gradient-like terms derived from invariant 
or non-invariant cost functions. The resulting nonlinear observers have 
strong (almost) global convergence properties and examples are used to 
demonstrate the relevance of the proposed approach. 
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1 Introduction 

There has been a surge of interest recently in nonlinear observer design for sys- 
tems on Lie groups. A driver for recent work is the growing demand for highly 
robust state estimation algorithms for autonomous robotic systems such as un- 
manned aerial, ground or submersible vehicles. Nonlinear observer design for 
such applications offers the potential of computationally simple state-estimation 
algorithms with strong robustness and global stability guarantees; as compared 
to the alternative of nonlinear filter designs (eg. extended Kalman filters [T] or 
particle filters [2]) that provide more information (posterior distributions for 
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the state estimates) but usually require significant computational resources and 
rarely have strong global stability or robustness guarantees. Previous work in 
nonlinear observer design for systems on Lie groups is often closely linked to 
specific applications. In the early nineties, Salcudean [3] proposed a nonlinear 
observer for the attitude estimation of a rigid-body using the unit quaternion 
representation of the special-orthogonal Lie group SO(3). This work is seminal 
to a series of papers that develop nonlinear attitude observers for rigid-body 
dynamics [U [7J [HI EE EI] , exploiting either the unit quaternion group struc- 
ture or the rotation matrix Lie-group structure of SO(3). The resulting attitude 
observers have comparable performance to state-of-the-art nonlinear filtering 
techniques [TT], generally have much stronger global stability and robustness 
properties, and are simple to implement. The full pose estimation problem has 
also attracted recent attention [121 E2 031 US] • In this case the underlying state 
space is the special Euclidean group SE(3) comprising both attitude and trans- 
lation of a rigid-body. Theoretical work in this direction is less advanced due to 
the more complex algebraic and geometric structure of SE(3). Recently, several 
authors have made a start in developing a theoretical foundation for observer 
design for general systems with invariance properties, and in particular systems 
with a Lie-group state space and the natural left invariant dynamics [161 117] . 
Early work in this direction considered uniform invariance properties across the 
system, measurements and observer design. More recent work recognises that 
it may be necessary to consider different invariance properties for the system 
than the measurements in order to obtain well conditioned observers |18j . Re- 
sults in this area are very recent and there is no well established observer design 
methodology for invariant systems on a Lie group, even in the case of full state 
measurement. 

In this paper, we study the design of non-linear observers for state space sys- 
tems where the state is evolving on a finite dimensional, connected Lie group. 
We consider the case where full measurements are available for the system kine- 
matics and provide a characterisation of invariant observer structures leading 
to an observer design methodology. Traditional full state observers, for sys- 
tems evolving on vector spaces, employ a design paradigm that goes back to 
the work of Kalman [TO] and Luenberger [5D] : The observer system is designed 
as a combination of a copy of the system or internal model (i.e. a part that 
can in principle replicate the observed system's trajectory), plus an innovation 
term which serves to drive the observer trajectory towards the correct system 
trajectory in the presence of initialization or measurement errors. We build on 
the results presented in [T51 [T7] to systematically study the invariance properties 
of internal models and innovation terms inherent in nonlinear observer design 
on Lie groups. We define the concept of synchrony of a plant /observer pair 
of systems (cf. also [3T]) with Lie-group state-space in terms of constant error 
evolution for a canonical invariant error term that is defined. This leads to a 
definition of internal model and a very general concept of innovation term for 
plant/observer systems on Lie groups. The internal model structure is shown 
to be a copy of the invariant plant dynamics. To design the innovation term, 
we utilise gradient like driving terms derived from algebraic cost functions on 
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the Lie group. Under mild assumptions, we prove almost global exponential 
convergence of the resulting observers and provide example derivations for the 
important applications of attitude and pose observer design for rigid-body kine- 
matics. Finally, we link gradient like dynamics of the canonical error back to 
a specific structure of the observer consisting of a synchronous term (internal 
model) and a gradient-like innovation term. Thus, the paper provides a coherent 
theory of nonlinear observer design for systems with left (resp. right) invariant 
kinematics on a Lie group for which one has a right (resp. left) invariant Morse- 
Bott cost function and full measurements. 

After the introduction, Section [2] provides an overview of the systems consid- 
ered and notation used. Section [3] introduces the concept of synchrony, defines 
left and right invariant errors, and proves some results concerning the canon- 
ical nature of invariant errors. Section [4] defines a concept of internal model 
and innovation term for systems on Lie groups. Section [5] goes on from the 
characterisation of observers to propose a specific design methodology based on 
using gradients of cost functions to define the innovation terms in the general 
observer structure defined in Section 3J Examples of observer designs on SO(3) 
and SE(3) are provided. The final technical section, Section [6l provides the link 
from gradient error dynamics back to the structure of the observer, confirming 
that the design methodology proposed in Section [5] is the only way to obtain 
the natural gradient error dynamics associated with a known cost function. A 
short paragraph of conclusions is also provided in Section [71 

A preliminary, short version of this paper has been accepted for publication 
at the MTNS 2008 1221. 



2 Notation and problem formulation 

Let G be a finite dimensional, connected Lie group with Lie algebra g. Denote 
the identity element in G by e, and left and right multiplication with an element 
X e G by L x and Rx, respectively. The tangent space T X G of G at X 
is represented by left or right translations of the Lie algebra, i.e. T e L X Q or 
T e RxQ- We use the simplified notation Xv for vectors T e Lxv € TxG and vX 
for vectors T e Rxv £ TxG with » £ g. Furthermore, we assume that there is 
a Riemannian metric (•, •) on G. Some of the results presented in the paper 
will depend on invariance properties of the metric, however, these will be stated 
explicitly where required and there are no general assumptions made regarding 
invariance of the metric. We use the norm ||i>|| 2 = {v, v) for v E g. 
Consider a left invariant system on G of the form 

X = Xu, (1) 

where u : M — > q is a function termed the input signal. An input u of sys- 
tem (JTJ is admissible if solutions of the system are unique, exist for all time 
and are sufficiently smooth. The system (JIJ has an equivalent right invariant 
representation 

X = vX (2) 
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with v: K — > g an admissible input. The input signals u and t> for left and right 
invariant representations of the same system are related by v — TxRx- 1 T e Lxu = 
Adx u. 

This paper discusses the design of observers that fuse potentially noisy mea- 
surements of X and u (resp. v) into an estimate of the state X. 

Example 1 Consider the design of an observer to estimate the attitude of a 
rigid-body [H EE [21 01 QZ^- The attitude of the rigid body is an element of 
the special orthogonal group SO (3), represented by real, orthogonal 3x3 matrices. 
The Lie algebra ofSO(3), denoted so(3), is the set of real, skew- symmetric 3x3 
matrices. The derivative of a curve R: K — » SO(3) coincides with its derivative 
in R 3x3 . The maps TrLs : R£l > SRfl and TrRs : RO, i— > RflS are given 
by left and right multiplication of the matrices in TrSO(3) with S, respectively. 
The tangent spaces T#SO(3) are identified with 

T R SO(3) = {Rn | fi € so(3)} C M 3x3 . 

The special orthogonal group has a bi-invariant Riemannian metric induced 
by the Euclidean metric on a the skew symmetric matrices, i.e. {RQ,RH) = 
tr(fTn). 

Consider the left invariant dynamics 

R = Rfl 

on SO(3) with fl: K — > so(3) admissible. This system models the kinematics of 
the attitude R of a coordinate frame fixed to a rigid body in 3D-space relative to 
an inertia! frame. Here, fl G SO (3) encodes the angular velocity ui S K 3 of the 
rigid-body measured in the body- fixed frame 

(0 -lu 3 lu 2 \ 
—UJl 
—oj 2 0Ji J 

In robotic applications, measurements of the attitude R are provided by ex- 
teroceptive sensor systems such as magnetometers, accelerometers and vision 
systems. The measurement of fl is typically obtained from on-board gyrometer 
systems. 

Example 2 The above example can easily be extended to model the full pose 
of a rigid-body, including the position p in 3D- space, by considering invariant 
systems on the special Euclidean group SE(3) \T3\ [T^J \T5jj . The special 
Euclidean group SE(3) has a representation as a semidirect product \23f o/SO(3) 
and R 3 

SE(3) = {(R,p) | R G SO(3),p G M 3 }, 
where the group product is given by 

(R,p)(S,q) = (RS,p + Rq). 
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Recall the shorthand notation 



T e L^ R p) (n v) = (r P )(n v) 



with (R pj G SE(3) and (f2 V) £ Se(3) and consider the left invariant system 



A(iz( t ) P (t)) = (ie P )(n v) = (izn w) 



witft f7 : M — s- so(3) cmd V^: M — * R 3 admissible. Here, V is the linear velocity of 
the rigid-body measured in the body-fixed frame. 

In robotic applications, the measurement of p is provided by exteroceptive 
sensor systems such as GPS (global positioning systems), radar or vision sys- 
tems. The measurement of linear velocity V is more challenging as there are few 
sensor systems that directly measure linear velocity. Typical measurement sys- 
tems depend on fusing differentiated exteroceptive position measurements with 
integration of accelerometer measurements. 

The two examples (Ex. [1] and [2|) provide an excellent means to demonstrate 
the structure of the design principles proposed in this paper and indicate the 
relevance of the results to real- world engineering problems. It is not the purpose 
of this paper, however, to enter into the details of the practical implementation 
of the proposed observers for a real world system and we will not consider issues 
such as choice of sensor systems or characterisation of noise any further than 
the brief discussion above and a couple of remarks later in the paper. 

3 Synchrony and error functions 

In this section, we introduce the general concept of synchrony between pairs 
of systems that evolve on a given Lie group G and have a common input. 
Loosely, synchrony refers to an equivalence, but not equality, of trajectories of 
two dynamical systems. More formally, we will define synchrony of two systems 
in terms of an error function E that quantifies the instantaneous difference 
between the system trajectories. The pair of systems is called E-synchronous if 
the error E is constant along trajectories. We show that synchronous invariant 
systems on Lie-groups have a particular structure and that the error function E 
associated with invariant synchronous systems is also structurally constrained. 
Consider a pair of systems on G driven by the same admissible input u 



with F X ,F X : GxqxR^TG, F x {X, u, t)eT x G and F x (X, u, t) e T X G. The 
solutions of ([3|) and J3J) are denoted by X(t; Xq, u) and X(t; Xq, u), respectively. 



X = F x (X,u,t), 
X = F x (X,u,t) 



(3) 
(4) 
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Definition 3 Consider the systems © and Let E: G x G — > M be a 

smooth error function, M a smooth manifold. The systems ([3]) and Q are said 
to be ^-synchronous if for all admissible u : K — > g, a/Z initial values Xq, Xq 6 G 
o/ © and g]) and all t G M 

^(l(t;Xo,«),X(t;X o ,u)) = 0. 

Two particularly simple error functions on a Lie group G are the canonical 
right invariant error 

E T {X,X):=XX- 1 (5) 
and the canonical left invariant error 

Ei(X,X) :=X- l X. (6) 

Where the arguments X and X are clear from the context we simply write E r 
and Ei . The label "invariant" refers to simultaneous state space transformations 
of both systems. That is, for all X,X,Se G, one has E r (XS, XS) = E r (X, X) 
and an analogous result for E\. 

Remark 4 Observe that both E r and Ei are non- degenerate in the sense that 
the partial maps E(X,-): G — > M and E(-,X): G — » M are global diffeomor- 
phisms (here, M — G). 

The next proposition shows that any error E for which two invariant systems 
are ^-synchronous, can be factored into one of the canonical invariant errors 
concatenated with a map from the Lie group into a manifold. Thus, the right 
and left invariant errors can be thought of as the fundamental error functions 
for pairs of invariant systems. 

Proposition 5 Consider the pair of invariant systems on G 

X = Xu, (7) 

X = Xu (8) 

for a single admissible input u: R — > g. Let M be a smooth manifold. Lf there 
exists a smooth error function E: G x G — > M such that the systems ([7]) and 
([5]) are E- synchronous, then E has the form 

E(X,X)=g{XX- 1 )=g{E r {X,X)) 1 

where g: G — > M is a smooth function. 

The analogous result holds for right-invariant systems 

X = vX 

X = vX. 

Ln that case one has 

E(X, X) = g{X- l X) = g{Ei{X, X)) 
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Proof: Consider the pair of systems ([7]) and © and an error function E as 
above. Let Xq,Xq,S £ G be arbitrary. Choose a smooth, bounded curve 
T: R — > G with bounded derivatives such that T(0) = e and T(l) = S 1 . 
Let (X, X) denote the solution of the pair of systems ([7]) and © for u(t) — 
T~ 1 (t)f(t) and (X, X)(0) = (X ,X ). Note that (X, X) (t) = (X T(t), X Q T(t)). 
The error function E is constant on the trajectory (X,X)(t) and therefore 
E(Xo,Xo) = E(XqS,XqS). Since Xo,Xo and S* are arbitrary, the function 
E is right invariant under the action of G, i.e. 

E = EoR s {qt all S £G. 

But this implies that E(X,X\ = EoR x - x (X,X) = E(XX-\e) for all 

G. Hence E has the form E(X, X) = where g: G — > M is the smooth 

function = E(Z,e). The other case is proven by an analogous argument. 

□ 

Given that the invariant errors Ei and E r play such a key role in the analysis 
of invariant synchronous systems it is of interest to specialise the notion of 
synchrony introduced above to the canonical errors. 

Definition 6 A pair of systems ([3|) and Q is termed right synchronous if they 
are E r - synchronous and left synchronous if they are Ei- synchronous. 

In the remainder of the section, we consider the structure of a pair of syn- 
chronous systems. We are interested in the case where the "first" system is left 
(resp. right) invariant (QJ (resp. |2])). We consider a "partner" system that is 
either left or right synchronous and derive constraints on the structure of the 
partner system. The result will provide us with the first half of the template for 
the design of non-linear observers. The proof of the main result (Theorem [S]) 
uses the following lemma. 

Lemma 7 Let X : R — > G and Y : R — > G be two smooth curves. Then 

= -T x -iR Y T e L x -±T x R x -iX + T Y L x -iY, 
YX 1 = TyRx-iY ^ Tx-iLyTeLx-iTxRx- 1 ^ . 

Proof: Recall that for the inverse function inv(X) = X^ 1 we have Tx inv = 
— Tx (Lx-iRx- 1 )- The equations for the derivatives now follow from the usual 
calculus rules for the multiplication on Lie groups. □ 

Theorem 8 Consider the left invariant system ([1} and let a second system be 
given by the general expression 

X = F x (X,u,t). (9) 
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The systems |T]) and © are right synchronous if and only if 

F x (X,u,t)=Xu. (10) 
The systems are left synchronous if and only if 

F x (X,u,t) = XAd x _ lx u = XAd E -i u. (11) 

Proof: By Lemma [7] we have for trajectories X(t) of (Q]) and X(t) of © 
that 

£V = T x R x -iX — T x -iL^ : T e Lx-iTxRx- 1 X 
= T x R x -iF x (X,u,t) 

— T x -iL x T e L x -iTxRx- 1 T e Lxu 
= T x R x -iF x (X,u,t) - T x -iL x T e R x -iu 

and 

Ei = —T x -iR x T e L x -iTxRx- 1 X + T x L x -iX 
= —T x -iR x TeLx-iTxRx- 1 TeLxu 

+ T x L x -iF x (X,u,t) 

= -T x -iR x TeRx-^u + T x L x -iF x (X,u,t). 

Hence E r is constant if and only if 

F x (X, u, t) — T e L x u = Xu. 

and Ei is constant if and only if 

F x (X,u,t) = T x _ lx L x T x -xR x T e R x -xu 

= T e L x Ad x - lx u = X Ad E -i u. 

□ 

In the case where the "first" system is right invariant the synchronous terms 
for the different errors are interchanged. 

Theorem 9 Consider the right invariant system © and let a second system 
be given by 

k = F x {X,v,t). (12) 
The systems and (TT2")) are left synchronous if and only if 

F x (X,v,t)=vX. 

The systems are right synchronous if and only if 

F X (X, v, t) = (Ad^ x _! v)X = (k&E r v)X. 
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It is interesting to observe that for a left invariant system |T]) and choos- 
ing the right synchronous constraint, one obtains left invariant dynamics for 
the partner system (JTUJ) . The dynamics (JTUJ) are highly desirable as the funda- 
mental component of a non-linear observer design since they are independent 
of the observed system's state X and yet ensure that the observer dynamics 
"track" the system state. This contrasts to choosing the left synchronous con- 
straint, where the resulting partner system structure (jTTJ) depends on the error 
Ei = X~ X X that requires knowledge of the observed system's state X and can- 
not be implemented in a real observer system. Thus, it is natural to consider 
right invariant errors and right synchronicity when designing observers for left 
invariant systems |T]); and vice- versa for right invariant systems 

4 Internal models and innovation terms 

In this section, we define the concept of internal model specialised for Lie group 
systems. This definition is used to show that any observer of an invariant system 
on a Lie group containing an internal model, can be split into a synchronous 
term, providing the internal model properties of the observer, and a second term 
that we identify as an innovation term. Thus, a natural approach to observer 
design is to choose the observer as a sum of a synchronous (internal model) 
term plus an innovation term, analogous to the "internal model plus innovation 
term" design paradigm for linear systems. 
Consider an observed system 

X = F x (X,u,t) (13) 

with Fx :Cxgxl-» TG, F X (X, u, t) e T X G and an observer 

X = F x (X,Y,w,t) (14) 

with F x : G x G x g KM. -y TG, F x (X,Y,w,t) 6 T X G. Note that the ob- 
server (1141) has two inputs Y and w, with Y to be fed with measurements of 
X and w to be fed with measurements of u, respectively. The idea behind an 
internal model is that the observer should be able to replicate the exact trajec- 
tory of the observed system if it is provided with exact input information; that 
is, the actual initial condition of the observed system's state and exact mea- 
surements. There is no a priori requirement that the system and the observer 
are identical dynamical systems, only that they correspond along certain very 
specific trajectories. 

Definition 10 Consider the pair of systems (IT31 and (fT¥|) . One says that (|14l) 
has an internal model of (|13l) if for all admissible u: K — > G, X$ £ G and all 

t S R 

X(t;X ,X(t;X ,u),u) = X(t;X ,u), (15) 

where X(t; Xq,u) and X(t; Xq,Y,w) denote the solutions of (|1 3|) and (|14[) . 
respectively. 
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Note that by Theorem [S] a right synchronous system for the left invariant 
system ([1]) has the form 

X = Xw (16) 

with w = u. It is straightforward to verify that (|16p has an internal model of 
the original left invariant system. The following result shows that any observer 
of a left invariant system containing an internal model can be split into two 
parts; the first or which is right synchronous with the original system, providing 
the internal model properties of the observer, and a second term that will be 
identified as an innovation term. 

Theorem 11 Consider the left invariant system ([T]) and the observer (I14[) . 
Assume that the observer has an internal model of the system. Then the right 
hand side of the observer can be written 

F jt (X,Y,w,t) = Xw + a(X,Y,w,t), (17) 

where a:GxGxgxW~^ TG is a smooth function satisfying a(X, Y, w, t) G 
TjrG and 

a (x(t; X ,X(t; X , u),u), X(t; X Q , u), u,tj =0 (18) 
for all admissible u: K — > g, Xq G G and igl. 

Proof: Define a(X, Y, w, t) = Fg(X, Y, w, i) — Xw for all X, Y G G, w 6 Q 
and t € K. Differentiating the internal model equation (fT5)) and using the system 
and observer equations yields 

F% (x(t;Xo,X(t-,Xo,u),u),X(t-,X ,u),u,t) = 

X{t; Xq, u)u 

for all admissible u: M —> g, Xq e G and t G R. We hence get 

a (Jt(t; X , X(t; X , u),u), X(t; X , u),u, tj = 

F k ^X(t;X ,X(t;X ,u),u),X(t;Xo,u),u,t S j - 

X(t;X ,X(i;X ,u),u)M = 

X{t;X 0l u)u - X(t;X ,X(t;X (h u),u)u = 

where the last equality follows again from the definition of an internal model. □ 

A similar result can obviously be obtained for right invariant observed sys- 
tems. 

The decomposition (|17p is analogous to the internal model plus innova- 
tion term decomposition of linear observers. The concept of right (resp. left) 
synchronicity provides a structural characterisation of internal models for left 
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(resp. right) invariant systems. The a term in Equation (fT7)) is analogous to the 
innovation term in classical linear observer design. Condition (fT8|) means that 
a is zero along corresponding trajectories of the system and the observer. 

Definition 12 Consider the pair of systems (|13|) and (|14p and assume that 
(|14p has an internal model of (|13p . FKe ca/Z a map a: GxGxgxl- > TG an 
innovation term «/ 

(CI) a(X, Y, w, t) G T ± G for all X, Y G G, w G Q, t G R and 

(C2) a(X(t; Xq, -X"(t; Xo, u), u), X(t] Xo, u),u,t) = for all admissible u : R — ► 
0, X € G and ieK. 

Note that the conditions specified in Definition [T^] are the least restrictive 
possible. In general, an innovation term must be chosen carefully to ensure that 
the trajectory of the observed system is an asymptotically stable limit set of the 
observer trajectory. We discuss design of the innovation term in Section [SJ 

Note further that Condition (C2) in Definition [T2l is in particular implied by 
the following stronger condition 

(C2') a(Y, Y, w, t) = for all Y G G, w G Q and t G R. 

Conditions (C2) and (C2') are in general not equivalent since there is no guar- 
antee that for a given admissible input u the trajectories of the system (and/or 
those of the observer) will cover all of G at any given time. 

In summary, we propose the following structure for the design of non-linear 
observers for invariant systems on Lie groups. 



This observer is intended for left invariant observed systems of the form ([T]), with 
Y receiving measurements of X and wi receiving measurements of u. Note that 
this observer is in general not left invariant, since a need not be left invariant. 



Correspondingly, this observer is intended for right invariant observed systems 
of the form ([2]) , with Y receiving measurements of X and w r receiving measure- 
ments of v. Note that this observer is in general not right invariant. 

5 Gradient observers 

In this section, we present an approach to choosing innovation terms for non- 
linear observers of the form (fT9)) and PD]) based on using the gradient descent 
direction of a suitable cost function. 

Let /: G x G ^ 1 be a smooth, non-negative cost function. Furthermore, 
let the diagonal A = {(X, X) \ X G G} consist of global minima of /. Recall 



X = Xwi + a(X, Y, wi,t) (left observer) 



(19) 



X = w r X + a(X, Y, w r ,t) (right observer) 



(20) 
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that the Riemannian gradient of / with respect to the product metric (•, -) p on 
G x G is defined by 

(grad f(X, Y), (77, 0) P = df(X, Y)(r,, C) 

for all X, Y 6 G, r\ € T^G, C € TyG. Since we use the product metric, the 
gradient splits into the gradients with respect to the first and second parameter, 
i.e. 

(grad/(X,y),(r ? ,C)) P - 

(gradi f(X, Y), n) + (grad 2 f{X, Y), 0- 

We propose to use the gradient of / with respect to the first parameter as 
an innovation term a in the design of observers. Given the observed system (fT]) 
(resp. ©) then the design of the observer is 

X = Xwi - gradi f{X, Y), (left observer) (21) 

respectively 

X = w r X - grad x f(X, Y). (right observer) (22) 

It remains to analyze the dynamics associated with this choice and show 
that the observer trajectory asymptotically converges to the observed system's 
trajectory. 

5.1 Error dynamics 

To understand the stability of the proposed observers (|21[) and (f2"2"]) we ana- 
lyze the case where exact measurements of the input u (resp. v ) of system (fTJ) 
(resp. ([2])) as well as exact measurements of the state X are available. In terms of 
the variables introduced in Section |4] one has Y — X and wi = u (resp. w r = v) 
in the case of the observer for a left (resp. right) invariant system. 
The following result is used in the development later in the section. 

Lemma 13 Let G be a Lie group. Let f : G x G — > K be a left invariant cost 
function and take a left invariant Riemmanian metric. Then for all X,Y, Z e G 

T x L Y -i gradi f(X, YZ) = g^ f{Y~ x X, Z) 

If f and the Riemmanian metric are right invariant, then for all X,Y, Z E G 

T x R z -i gradi f(X, YZ) = gmd, f{XZ-\Y) 

Proof: If / is left invariant, then f oLy — f ■ Using this fact and the standard 
rules for transformations of Riemannian gradients, we have that 

gradi f(Y-'X, Z) = grad^/ o Ly^Y^X, Z) 

- (iy-i x Z r )* f(X,YZ), 



12 



where (Ty-ix^y)* denotes the Hilbert space adjoint of the linear map T y -\ x Ly- 
Since the Ricmannian metric is left invariant, we have that (Ty-ixLy)* — 
TxL Y -i- The right invariant case follows from an analogous argument. □ 

To analyze the asymptotic stability of the observer trajectory to the ob- 
served system's trajectory it is convenient to consider the dynamics of the error 
functions E r and Ei and prove that their trajectories converge to the identity 
element of the Lie group. Since E r and E\ are non-degenerate, this will imply 
the desired asymptotic stability. In particular, under suitable invariance condi- 
tions on the cost function and the Riemannian metric the error dynamics for 
the "correct" invariant error are autonomous. 

Theorem 14 Consider the left invariant system ([I]). Let f : G x G — » M be a 
right invariant cost function and take a right invariant Riemannian metric on 
G. Consider the left observer dynamics (|2ip . Then the error dynamics of the 
right invariant error E r §5§ are given by 

E r = -gradj f(E r ,e). 

If the right invariant system ^ is considered with a left invariant cost func- 
tion and Riemannian metric along with the right observer dynamics (|22p . then 
the error dynamics of the left invariant error E\ ([6]) are given by 

E l =-gra,d 1 f(E l ,e). 

Proof: Directly from the system equations and the equations of the left 
observer it follows that 

E r = -T jt R x -i B rad l f(X,Y) 
= -T jt R x -i B rad l f(X,X). 

Using Lemma [T3] and the right invariance of / and the Riemannian metric, we 
get that 

E r = - gradi f(XX-\ e) = - gradi f(E r , e). 
The result for the right observer follows from an analogous argument. □ 

The gradient dynamics of the error yield the following convergence result in 
the noise-free case. 

Theorem 15 Assume that Y t— * f(Y,e) is a Morse-Bott function with a global 
minimum at e and no other local minima. If f and the Riemannian metric on 
G are both right invariant, then both errors E r and E\ of the left observer (|2ip 
converge to e for generic initial conditions. Furthermore, f(E r ,e) converges 
monotonically to /(e,e) and this convergence is locally exponential near e. If f 
and the Riemannian metric on G are both left invariant, then both errors E r 
and Ei of the right observer (|22[) converge to e for generic initial conditions. 
Furthermore, f(Ei,e) converges monotonically to /(e,e) and this convergence 
is locally exponential near e. 
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Proof: Let us consider the left observer. The gradient structure of the 
error dynamics (Theorem [M]) and the conditions on the cost function yield the 
monotonic convergence of f(E r ,e) and therefore the convergence of E r to e. 
As / is a Morse-Bott function, the convergence of f(E r ,e) is exponential near 
e. Since convergence of E r to e implies the convergence of Ei to e, we get the 
convergence of the left invariant error, too. The result for the right observer is 
proved by an analogous argument. □ 



Remark 16 Consider the left invariant system ([T]) and noisy measurements 
wi = u+5 and Y = NiX, with additive driving noise 6 £ g and left multiplicative 
state noise Ni G G. A straightforward calculation yields 

E r = Ad^ SE r - gradi f(E r , JV,) 

for the canonical right invariant error of the left observer (|2ip . There is an anal- 
ogous formula for the canonical left invariant error of the right observer (I22p . 
this time best expressed in terms of right multiplicative state noise Y = XN r . 
It is intuitively clear that suitably bounded noise will yield at least a practical 
stability result in these cases. 

Theorems and [TS] are the main results of this section. Together, these 
results provide a template for the design of non-linear observers for invariant 
systems on a Lie-group, Equations (f2Tj) and (|22|) . There is still an outstanding 
question of how to find suitable cost functions that we will address in Section 
15.31 In the remainder of this section, we consider some of the special cases that 
were not addressed in Theorems [U] and Qj)] 

The convergence result in Theorem [TS] raises the question of what the dy- 
namics of the other error, i.e. Ei for the left observer and E r for the right 
observer, looks like and if its cost converges monotonically to /(e,e). Under 
suitable invariance conditions, one obtains the following result on the dynamics 
of the other errors for each observer. 

Theorem 17 Consider the left invariant system ([1]). Let f : G x G — > K be a 
left invariant cost function and take a left invariant Riemannian metric on G. 
Consider the left observer dynamics (I21[) . Then the error dynamics of the left 
invariant error Ei are 

E x = (T e L El - T e R El )u - gradi f{E h e) 
= {Em - uEi) - gradj f(E h e). 

If the right invariant system @ is considered with a right invariant cost 
function and Riemannian metric along with the right observer dynamics (|22l) . 
then the error dynamics of the right invariant error E r are 

E r = (T e R Er - T e L Er )v - gradi f(E r , e) 
= (vE r - E r v) - grad x f(E r , e). 
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Proof: Let us consider the left observer. Using Lemmas [7] and [T3] one 
obtains 



Ei — —Tx-iRxT e Lx-iTxRx- 1 TeLxu + T-^L x -iT e L-^u 

= {T x L x -±T e L x - T x -iRxT e R x -^)u 

= {T e L El - T e R El )u - grad-i f{E u e). 
The statement for the right observer follows from an analogous calculation. □ 

It is interesting to consider the case of a bi-invariant cost function /. In 
this case it is possible to show that the non-gradient term in the error dynamics 
(Theorem 1 17p is passive with respect to the cost, i.e. an element of the kernel 
of the differential of the cost. 

Lemma 18 Consider a Lie-group G with Lie-algebra q. Assume that there 
exists a bi-invariant cost f : G x G — > R. Then for all u,v G Q and any 
Ei,E r € G 

{{T e L El ~ T e R El Kgradi f{E u e)) = 
{{T e R Er — T e L Er )v, grad-t f(E r , e)) = 

Proof: For all u S g we have by the bi-invariance of / and standard 
transformation rules for the gradient that 

(T e L El u,ffad 1 f(E l ,e)) = (u, (T e L El )* grad x f(Ei,e)) 
= (u,grad 1 /(e,£;f 1 )) 
= (u,(T e R Ei y gradi f{E u e)) 
= (T e R El u,gYad 1 f(E l ,e)). 

This yields the hrst equation. The other equation follows from an analogous 
argument. □ 



Proposition 19 Consider the left invariant system ((T|). Let / :GxG^l be 
a bi-invariant cost function and take a left invariant Riemannian metric on G. 
Consider the left observer dynamics (|21[) . Then the time derivative of the cost 
function along trajectories of (JXJ) and (PHI) is given by 

A/^-Hgrad^i^e)!! 2 . 

The analogous result holds for a right invariant Riemannian metric on G 
and the right observer dynamics (|22p. 
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Proof: Consider the left invariant case. The differential of / is given by 

^/ = (gradi f(E u e), (T e L El - T e R El )u - gradi f{E h e)) 

= -(grad 1 /(^,e),grad 1 /(^;,e)) 
= -||grad 1 /( J B i ,e)|| 2 

by applying Lemma 1181 The analogous result for right invariant dynamics is 
similarly direct. □ 

The above result can be developed into a convergence result analogous to 
Theorem [151 In practice, the case that is of most interest is when both the cost 
function and the metric are bi-invariant, a situation that occurs for the attitude 
estimation example on 5*0(3). In this case the following convergence result is 
obtained. 

Corollary 20 Consider the left invariant system ([1]) and the right invariant 
system Assume that G admits a bi-invariant metric and let f: G x G — > K 
be a bi-invariant cost function. Assume that Y \— * f(Y, e) is a Morse-Bott 
function with a global minimum at e and no other local minima. 

For both the left and right observers (|21|) and (|22|) then f(E r , e) and f(Ei, e) 
converge monotonically to /(e, e) for generic initial conditions. The convergence 
is locally exponential near e. 

Proof: The convergence of the cost of one type error for each filter is shown 
in Corollary 1151 The convergence of the other error follows from the structure 
of the error dynamics, cf. Theorem 1 171 and Proposition [TO] by a straightforward 
Lyapunov argument. □ 



5.2 Example: Attitude estimation on S0(3) 

We revisit Example [T] on the special orthogonal group SO (3). The left invariant 
attitude kinematics are 

it = RCl, 

where R denotes the attitude of a coordinate frame fixed to a rigid body in 3D- 
space relative to an inertial frame and f2 encodes the angular velocity measured 
in the body-fixed frame. The velocity measurements are given by wi = Q and 
the state measurements are given by Y = R. The right invariant and left 
invariant errors have the form E r = RR T and E\ — R T R, respectively. 

We define the cost function f(R,Y) = |||.R-Y||§,, with || • \\ F the Frobenius 
matrix norm and k a positive constant. Since the Frobenius norm is invariant 
under orthogonal transformations it follows that / is bi-invariant. Moreover, the 
standard Ricmannian metric on SO(3), induced by the Euclidean inner product 
on K 3x3 restricted to the Lie-algebra of skew symmetric matrices, is also bi- 
invariant. Let us recall the well-known methods to calculate the gradient of 
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R i ► f(R 7 Y), see e.g. [23]. As we use the induced metric on SO(3), the gradient 
is given by the orthogonal projection of the Euclidean gradient in R 3x3 to the 
tangent space of SO(3). This projection to T^SO(3) can be readily calculated 
to be Z RF(R T Z), where P denotes the orthogonal projection onto the skew- 
symmetric matrices, that is ¥(Z) — ^(Z — Z T ). Since the Euclidean gradient of 
R i-s- f(R, Y) is just k(R—Y), this yields that grad x f(R, Y) = kRP(I-R T Y) = 
-kRP(R T Y). Hence we have the observer 

R = Rw t + kRF(R T Y) (left observer) 
which coincides with the passive filter 

R = M + kRP(R T R) 

proposed in [8] [6] (recall that wi = f2 and Y = R in the noise free case). 
Starting with right invariant attitude kinematics 

R = TR 

and measurements w r — T and Y — R, we have the observer 

R = w r R + kRP(R T Y) (right observer) 

which, using T = Adn f2, coincides with the direct filter 

R = Ad R nR + k Ad k ¥(R T R)R 

discussed in [8] [6] but also extensively studied over the last ten years by a range 
of authors [1 U ED] • 

5.3 Construction of invariant cost functions 

In this section we investigate the question of finding invariant cost functions on 
Lie groups. 

For a left invariant system, Theorem[T3]shows that the canonical right invari- 
ant error for the left observer has gradient dynamics if the cost and Riemannian 
metric are right invariant. A right invariant Riemannian metric can be easily 
constructed on any Lie group by transporting a scalar product on the Lie al- 
gebra to other tangent spaces by right translation. However, it is a priori not 
clear how to obtain a right invariant cost function, in particular if the group 
is non-compact. For example, the most natural cost function on SE(3) (cf. 
Example [2j would be 

f((R,p),(Y,y)) = \\R-Y\\ 2 + \\p-y\\ 2 , (23) 

however, it is easily verified that this cost function is not right invariant. It 
should be noted that finding a Morse-Bott cost function is usually fairly straight- 
forward, (123|) is an example of such a function, the challenge lies in ensuring 
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the function chosen has the desired invariance properties. The next proposition 
provides a method to obtain an invariant cost function on a Lie group given 
that a suitable Morse-Bott function on G is available. 

Proposition 21 Let G be a Lie group and let g : G —> R be a smooth function. 
Then /: G x G — > K, f(X, Y) = g(XY^ 1 ) is a smooth, right invariant function. 
Furthermore, if g is a Morse-Bott function with a unique global minimum at e 
and no further local minima, then Y t—> f(Y, e) is Morse-Bott with a global 
minimum at e and no further local minima. 

Proof: The smoothness of / is obvious. For all X,Y, Z S G we have 
f(XZ,YZ) = giXY- 1 ) = f(X,Y) and thus / is right invariant. The sec- 
ond statement follows directly from f(Y, e) — g(Y) for all feG. □ 

An analogous construction yields a left-invariant cost function. Furthermore, 
we can obtain a right invariant cost function from any left invariant cost function 
and vice- versa from the following result. 

Proposition 22 Let G be a Lie group and let f : GxG^l be a left invariant 
function. Then f : G x G — ► M defined by 

f{X,Y) = f{X-\Y- 1 ) 

is a right invariant function. Furthermore, if Y >— > f(Y, e) is a Morse-Bott 
function such that e is the only local minimum and a global minimum, then 
Y i ► f(Y, e) has the same properties. 

Proof: The right invariance is checked by a straight forward calculation. The 
second statement follows from the fact that the map inv: G — > G, X X^ 1 is 
a global diffcomorphism. □ 



5.4 Example: Pose estimation on SE(3) 

As an application of the cost function construction above, let us recall Example 
[2 the special Euclidean group SE(3). The system on SE(3) is given by 

±(R(t) P (t)) = (m RV), 

where R resp. p are the attitude resp. position of a coordinate frame fixed to 
a rigid body in 3D-space relative to an inertial frame, Q denotes the angular 
velocity and V denotes the linear velocity measured in the body-fixed frame. As 
mentioned before, the natural cost function (|23[) on SE(3) is not right invariant, 
and hence Theorem [15] cannot be applied. However, Proposition [21] yields a 
construction procedure for a right invariant cost function. For this we need to 
choose a suitable function g on SE(3). We use 

g(R,p) = ±(\\R-L\\i + \\ P r). 
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It is straightforward (although tedious) to verify that g is a Morse-Bott function 
with a unique global minimum at (/, 0) and no further local minima. Proposition 
[H] yields the right invariant cost function 

f((R,p),(Y,y)) = gURM^y)- 1 ) 

= l(\\R-Yr + \\p-RY T y\\ 2 ). 

Note that 

f((R,p), (Y,y)) = \ (||i? T - F T || 2 + || - R T p (-r T y)|| 2 ) . 

Hence we can view / also as the result of applying Proposition [22] to the cost 
function (|23|) . In order to construct a left observer, such that Theorem [T5l 
can be applied, we also need a right invariant Riemannian metric. Note that 
T [Jt0) R (A ^(n,V) = {nR,V + np) for all {R,p) 6 SE(3) and (Q.,V) € «(3). 
Hence we can define a Riemannian metric by 

((QiR, Vi + nip), (n 2 R, v 2 + n 2 p)) = tr{njn 2 ) + v?v 2 T 

for all (fliR, Vi + Oip), (fl 2 R, V 2 + Q 2 p) E TU #) SE(3). Note, that we have used 
the representation of tangent vectors by right translation of the Lie algebra in 
this definition. For our filter we have to calculate the gradient with respect 
to the Riemannian metric. As a first step we derive a closed formula for the 
differential of / with respect to the first variable. 

dxf((R,p), (r, y))(QR, v + rip) = - tr(nRY T ) + {p- ry t v , v) 

where P again denotes the orthogonal projection onto the skew-symmetric ma- 
trices. Hence we have the following formula for the gradient, 

= T (Ifi) R {Ap) (P(RY T ) 7 p-RY T y) 

= T (iV) L (R,p) Ad (fl,p)- (nRY T ),p RY T y). 

To obtain a representation of gradj / as left-translated elements of se(3) we 
check that 

Ad iA>pyl (F(RY T ),p-RY T y) = (P(Y T R), R T p - Y T y + R T P(RY T )p) 

Assume now that we measure the angular and linear velocities (wn,wy) — 
(fi, V) and the system state (Y,y) — (R,p). Using the construction above we 
get the left filter 

R = Rw n - RF(Y T R) 

f> = Rw v - (p - RY T y) - ¥(RY T )p. 
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By Proposition[5TJ and since g was a suitable Morse-Bott function, we can apply 
Theorem [15] and see that the right invariant error for this observer converges to 
the identity for generic initial values. 

To the best of our knowledge, the above observer on SE(3) has not been 
proposed in the literature before. Previous work by two of the authors [H] 
yielded an observer on SE(3) with exactly the same structure, but with the last 
term in the p-equation, i.e. — P(RY T )p, replaced by — RY T P(RY T )y. Moreover, 
Lyapunov-type stability results were proven in that paper for the observers 
including or not including that term, respectively. 

6 Gradient-like observers 

In this section we relax the invariance requirements placed on the cost function 
we have used to design the innovation term in our gradient observers and con- 
sider general cost functions. To provide structure for the observer design we 
require that the error dynamics exhibit autonomous gradient dynamics. This 
approach leads to a version of an internal model principle for invariant systems 
on a Lie group. The principle states that observers for invariant systems that 
exhibit gradient dynamics for the canonical invariant errors will contain an in- 
ternal model of the observed system and can be decomposed into a synchronous 
term plus an innovation term analogous to that discussed in Section 2J The 
innovation term is not itself a gradient term unless the cost function has the 
invariance properties discussed in Section [5] and we term the resulting observers 
gradient-like. This result sharpens a related structural result in [T7] (see also 
|16j). where less stringent conditions were placed on the error dynamics. Sharp- 
ening these requirements allows us to derive an almost global convergence result 
in a very general context. 

Theorem 23 Let f : GxG^l be a smooth cost function. Consider a general 
left observer for system ((T|), 

X = F jt {X,Y,w l ,t) (24) 

with measurements Y = X and wi — u. Then the canonical right invariant 
error E r displays gradient dynamics 

E r = -grad 1 /( J B r ,e), (25) 

if and only if 

Fx(X,Y,w u t) =X Wl -T i:y _ lJ R r grad 1 /(lY- 1 ,e). 

The analogous result holds for right observers for system ([2]) with measure- 
ments Y = X and w r = v, and the canonical left invariant error Ei . In this 
case, the dynamics of the left invariant error are gradient dynamics 

Ei=-ecad 1 f(E l ,e), (26) 
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if and only if 

F X (X, Y, w r , t) = w r X - T Y _ 1± L Y gradi f(Y~ 1 X, e). 

Proof: Assume that we have an observer (j2"4"]l with error dynamics ([23)) . 
We can split F X (X, Y, wi,t) = Xwi + a into the synchronization term Xwi and 
a remainder term a by defining a = F X (X ,Y,wi,t) — Xwi. By Lemma [7] and 
using the same argument as in the proof of Theorem as well as Wt = U, we 
see that 

E r = T x Rx-±ot. 

We see that a = —T XY _ l R Y gradj f(XY^ l ,e) by using X = Y and f2"5]). In 
particular, a is an innovation term in the sense of Section 3J If on the other 
hand F X (X,Y, wi,t) has the form as given in the Theorem then a straightfor- 
ward calculation shows that we get the error dynamics l|25p. The statement for 
the error dynamics (f2l>)) follows analogously. □ 

Theorem [23l yields the two gradient-like observers 

X = Xw l -T XY _ 1 R Y grad 1 f(XY- 1 ,e) (27) 

and 

X = w r X - T Y _ 1X L Y grad x f{Y~ x X, e). (28) 

Note that by Lemma [13] these observers coincide with the gradient observers 
(|2T|) and |22|) . respectively, if the cost function and the Riemannian metric are 
right, respectively left, invariant. 

Corollary 24 Assume that Y i— > /(K, e) is a Morse-Bott function with a global 
minimum at e and no other local minima. Both errors (E r and E{) of the left 
observer (|27[) and both errors of the right observer (|28p converge to e for generic 
initial conditions. Furthermore, f(E r ,e) converges monotonically to /(e,e) for 
the left filter and f(Ei, e) converges monotonically to f(e, e) for the right filter, 
and this convergence is locally exponential near e in both cases. 

Proof: The proof is entirely analogous to that of Theorem fTS] □ 



Remark 25 In the case of noisy measurements wi = u + 5 and Y = XN r , a 
straightforward calculation shows 

E r =M X 8E r - T ErN -iR x -i NTX gradi f(E r N~\e) 

for the canonical right invariant error of the left gradient-like observer (|27[) . 
The formula for the canonical left invariant error of the right gradient-like ob- 
server (|28p is analogous. It is intuitively clear that these systems will have 
strong practical stability properties. 
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7 Conclusion 



This paper provides a coherent theory of nonlinear observer design for systems 
with left (resp. right) invariant kinematics on a Lie group for which one has a 
right (resp. left) invariant, non-degenerate, Morse-Bott cost function and full 
measurements. The key contributions are the observer equations (|2"Tj) and (|22[) 
along with Theorems [TJ] and [15j The results in Section 15.31 are of practical 
importance in generating invariant cost functions. Finally, the results presented 
in Section [6] provide a practical design methodology in the case where a non- 
invariant cost function is considered. A limitation of the approach described in 
this paper is the requirement for full measurement of both state and velocity. In 
work in progress, we are investigating the structure of observers for kinematic 
systems on Lie groups with partial state measurements. 
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